#include "DataIO/GroundTruthReader.h"
#include "DataIO/DataReader.h"
#include "util/TIMER.h"

#include "DataStructure/Frame.h"
#include "DataStructure/LandMarkPLX.h"
#include "DataManager.h"

#include <unordered_map>
#include "ceres/PBAFactors.h"
#include "initialization/Initialization.h"
#include <Eigen/Core>
#include <tuple>

#include "Optimizer.h"
#include "StepsOptimization.h"

// #include "SystemData.h"

using namespace Estimator;


int main(int argc, char** argv)
{
    GroundTruthReader gtReader(argv[1]);
    Estimator::DataReader dataReader(argv[1]);    // here just for observations

    gtReader.PrintGT();

    std::vector<Estimator::DataReader::PoseType> poseContainer = dataReader.GetPoseContainer();
    auto FrameContainer = gtReader.mFramePosesGroundTruth;

    std::cout << "Get Frame of data Reader." << poseContainer.size() << std::endl;
    

    // #pragma region read data from files. Translation vector of all frames are come from the original txt file. Rotation matrices is read from ground truth file.
    std::cout << "=========== Generate data associations from TXT files  ============" << std::endl;
    std::vector<Frame> vFrames;
    vFrames.reserve(2000);
    for(size_t pose_id = 0; pose_id < FrameContainer.size(); pose_id++)
    { 
        // std::cout << FrameContainer[pose_id][0] << " "; 

        Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Rot; 
        // all rotation is filled in with groundtruth.
        Rot << FrameContainer[pose_id][0], FrameContainer[pose_id][1], FrameContainer[pose_id][2],
               FrameContainer[pose_id][4], FrameContainer[pose_id][5], FrameContainer[pose_id][6],
               FrameContainer[pose_id][8], FrameContainer[pose_id][9], FrameContainer[pose_id][10];

        // Vector3d should be with ground truth.
        Eigen::Vector3d vec = Eigen::Vector3d(FrameContainer[pose_id][3], FrameContainer[pose_id][7], FrameContainer[pose_id][11]);

        std::cout << "pose_id: " << vec[0] << " " << vec[1] << " " << vec[2] << std::endl;
        // Frame frame = Frame(pose_id, )
        auto obsRange = dataReader.FindObservationRangeByFrameID(pose_id);
        Frame frame = Frame(pose_id, Rot, vec);
 
        
        for(auto ite = obsRange.first; ite != obsRange.second; ++ite)
        {
            frame.mvLandMarkObservations_.emplace_back((*ite).first.second, (*ite).second);        
        }

        vFrames.push_back(frame);
    }
    // for(size_t landmark_id)
    // #pragma endregion

    
    std::cout << "========== Generation completed ================" << std::endl;


    Calibration calibration;
    calibration.Rbc = Eigen::Matrix3d::Identity();
    calibration.pbc = Eigen::Vector3d::Zero();
    calibration.K = Eigen::Matrix3d::Zero();
    // Liang's data: 
    // calibration.K(0,0) = 923.5295;
    // calibration.K(1,1) = 922.2418;
    // calibration.K(2,2) = 1;
    // calibration.K(0,2) = 507.2222;
    // calibration.K(1,2) = 383.5822;


    // Teng's data: 
    calibration.K(0,0) = 525;       
    calibration.K(1,1) = 525;
    calibration.K(2,2) = 1;
    calibration.K(0,2) = 639.5;
    calibration.K(1,2) = 479.5;
    calibration.Kinv = calibration.K.inverse();

   
    Managing Manager(&calibration);

    std::cout << "=========== Add Frames to the Manager ==============" << std::endl;

    for(size_t i = 0; i < vFrames.size(); ++i)
    {
        Manager.AddFrame(&vFrames[i]);
    }

    Manager.RemoveLandMarkWithSingleAnchor();
    Manager.SetInitializationParameters();
    Manager.SimplyOutPutParallax("./GL");
    std::cout << "Manger's landMark : " << Manager.mlLandMarkContainer_.size() << std::endl;
    std::cout << "Manger's Poses : " <<  Manager.mvpFrameContainer_.size() << std::endl;
    
   
    // std::cout << "========================== Full PBA solve=================" << std::endl;
    // Optimizer optimizer(calibration);
    // optimizer.InitializeByManager(&Manager);
    // Manager.AddNoiseToRotations();
    // optimizer.GenerateCostFunction(&Manager);
    // optimizer.SolveOptimization();
    StepsOptimization stepOptimization(calibration);
    stepOptimization.RunOptimization(&Manager);

}

